package dk.rhok.findstufffast;



import android.hardware.*;
import android.content.*;
import java.util.*;



public class Compass implements SensorEventListener
{
    private float[] lastMagnetic;
    private float[] lastAcceleration;
    private CompassListener listener;
    private SensorManager sensorManager;

    private final int HISTORY_MAX_LENGTH = 9;
    private final int AVG_LENGTH = 5;
    private int historyNextIdx;
    private float[] directionHistory;
    private float[] directionHistoryCopy;



    public Compass(Context context)
    {
        sensorManager = (SensorManager)context.getSystemService(Context.SENSOR_SERVICE);

        sensorManager.registerListener(this,
                                       sensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD),
                                       sensorManager.SENSOR_DELAY_FASTEST);

        sensorManager.registerListener(this,
                                       sensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER),
                                       sensorManager.SENSOR_DELAY_FASTEST);

        historyNextIdx = 0;
        directionHistory = new float[HISTORY_MAX_LENGTH];
        directionHistoryCopy = new float[HISTORY_MAX_LENGTH];
    }



    public void onAccuracyChanged(Sensor sensor, int accuracy)
    {
    }



    public void onSensorChanged(SensorEvent event)
    {
        if(event.sensor.getType() == Sensor.TYPE_MAGNETIC_FIELD)
            lastMagnetic = event.values;
        if(event.sensor.getType() == Sensor.TYPE_ACCELEROMETER)
            lastAcceleration = event.values;

        if(lastMagnetic == null || lastAcceleration == null)
            return;

        float[] R = new float[16];
        float[] I = new float[16];

        sensorManager.getRotationMatrix(R, I, lastAcceleration, lastMagnetic);
        float[] orientation = new float[3];
        sensorManager.getOrientation(R, orientation);

        double dirMath = orientation[0];
        double dirDeg = -180.0 * dirMath/Math.PI;

        directionHistory[historyNextIdx++] = (float)dirDeg;
        if(historyNextIdx >= directionHistory.length)
            historyNextIdx = 0;

        if(listener != null)
            listener.onCompassChanged();
    }



    public float getLastDirection()
    {
        System.arraycopy(directionHistory, 0, directionHistoryCopy, 0, HISTORY_MAX_LENGTH);
        Arrays.sort(directionHistoryCopy);

        float sum = 0;
        int offset = (HISTORY_MAX_LENGTH - AVG_LENGTH)/2;
        for(int i = 0; i < AVG_LENGTH; i++)
            sum += directionHistoryCopy[i + offset];

        return sum/AVG_LENGTH;
    }



    public void setListener(CompassListener listener)
    {
        this.listener = listener;
    }
}